clc;
clear;
close all;

joint_num = 6;

a_sym = sym('a_%d',[joint_num, 1],'real');
d_sym = sym('d_%d',[joint_num, 1],'real');
alpha_sym = sym('alpha_%d',[joint_num, 1],'real');
q_sym = sym('q_%d',[joint_num, 1],'real');

% 确定标准DH Table中的常数项
a_sym(1) = 0;a_sym(4) = 0;a_sym(5) = 0;a_sym(6) = 0;
d_sym(2) = 0;d_sym(3) = 0;
alpha_sym(1)=pi/2;alpha_sym(2)=0;alpha_sym(3)=0;alpha_sym(4)=pi/2;alpha_sym(5)=-pi/2;alpha_sym(6)=0;

for i=1:6
    T = [
        [cos(q_sym(i))], [-sin(q_sym(i))*cos(alpha_sym(i))], [sin(q_sym(i))*sin(alpha_sym(i))], [a_sym(i)*cos(q_sym(i))];
        [sin(q_sym(i))], [cos(q_sym(i))*cos(alpha_sym(i))], [-cos(q_sym(i))*sin(alpha_sym(i))], [a_sym(i)*sin(q_sym(i))];
        [0], [sin(alpha_sym(i))], [cos(alpha_sym(i))], [d_sym(i)];
        [0], [0], [0], [1]];
    
    if (i==1)
        T_bi(:, :, i) = T;
    else
        T_bi(:, :, i) = T_bi(:, :, i-1)*T;
    end
end

% 质心位置
xc_sym = sym('xc_%d',[joint_num, 1],'real');
yc_sym = sym('yc_%d',[joint_num, 1],'real');
zc_sym = sym('zc_%d',[joint_num, 1],'real');

% 连杆质量
m_sym = sym('m_%d',[joint_num, 1],'real');

% 6连杆臂
% ∂T_06 / ∂q6
J_66 = T_bi(:, :, 1);
J_55 = J_66;
J_65 = J_66;

for (k=1: joint_num)
    for (i=1: 4)
        for (j=1: 4)
            J_66(i, j) = jacobian(T_bi(i, j, joint_num), q_sym(joint_num));
            J_55(i, j) = jacobian(T_bi(i, j, joint_num-1), q_sym(joint_num-1));
            J_65(i, j) = jacobian(T_bi(i, j, joint_num), q_sym(joint_num-1));
        end
    end
end

% 重力加速度
g = [0, 0, sym('g', 'real'), 0];

% 
tg_6 = simplify(-m_sym(joint_num)*g*J_66*[xc_sym(6), yc_sym(6), zc_sym(6), 1]');
tg_5 = simplify(-m_sym(joint_num-1)*g*J_55*[xc_sym(5), yc_sym(5), zc_sym(5), 1]'...
                -m_sym(joint_num)*g*J_65*[xc_sym(6), yc_sym(6), zc_sym(6), 1]');

